/**
 * @file sendcmd.h 
 * @author Julien Mathieu
 * @brief Defines the class sendcmd, which wil send the command to te lowlevel and it will calculate if the robot is on the correct place after sending the commands
 */

#ifndef SENDCMD_H
#define SENDCMD_H

#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "../lowLevel/LowLevel.h"
#include "../wifi/RobotData.h"
#include "Point.h"
#include "../wifi/Client.h"
#include <vector>
#include "CmdGenerator.h"

#define	HOEKPERSEC 80
#define PIXPSEC 100
//#define TIME 4.5
#define LSPEED 100
#define RSPEED 100

/**
 * @brief send the commands to the lowlevel and checks if the correct position is reached 
 */
class sendcmd{
	public:

		/**
		 * Constructor
		 */
		sendcmd(RobotData *rob, Client *client);

		/**
		 * Destructor
		 */
		~sendcmd();
		
		/**
		 * Will convert the distance and angle to the specifications of the robot speed.
		 * It wil send a command to move a specific direction and after a calculated time it will send a stop command.
		 *
		 * @param	int afstand
		 *		the distance that wil be converted.
		 * @param	float hoek
		 * 		the angle that wil be converted.
		 */
		void drive(int afstand, float hoek);

		/**
		 * Will Send diference command, 0 = rotate left, 1 = rotate right, 2 = move forward and 3 = mover backward.
		 * after a precalculated time it will send a stop command.
		 *
		 * @param	int tijd
		 *		the time it will be executing the command.
		 * @param	int geval
		 * 		the 4 difference command that it can call.
		 */
		void stuurcomd(int tijd,int geval);

		/*
		 * When the cmd is finished it wil controlate if it has reached the correct position
		 */
		bool controleernabereikt(void);
		
		/**
		 * This function wil controle if the reached position is the same as the wanted position
	 	 *
		 */
		void controlleerpositie(int tijd);
		/**
		 * This function wil controle if the reached angle is reached and stop turning
		 */
		void controlleerdraai(int tijd);

		/**
		 * This function send to the lowlevel that the grab must be closed
		 */
		void closegrab();

		/**
		 * This function send to the lowlevel that the grab must be opend
		 */
		void opengrab();
		

	private:
		LowLevel *call;		//functions of lowlevel
		int robotd_x;		//destination x of robot	
		int robotd_y;		//destination y of robot
		float robotd_angle;	//destination angle of robot
		RobotData *robot;	//robot
		CmdGenerator *cmdgen;		//generator om nieuwe commandos te sturen wanneer niet op juiste plaats staat
		Client *WifiClient;	//data van de wifi dat binnenkomt
		RobotData *nutteloos;
		vector<Point> obstakels;
};

#endif
